#ifndef _HDR_PFINDER
#define _HDR_PFINDER

#include <cstdio>
#include <list>
using std::list;

#define USE_INTERNAL_CONTAINER  true    //Compilation avec les conteneurs par défaut
#define PFINDER_DEBUGMODE       false    //Compilation avec fonction de débuggage (peu de chances que cela fonctionne sans les conteneurs de base)

#if PFINDER_DEBUGMODE
#define PF_DFT_DEBUG_FDESCR     stdout  //Descripteur de fichier par défaut du débug
#define PFINDER_TRACK_ITERATION true    //Retient le numéro de l'itération (nécessaire pour les dumps de listes)
#define PFINDER_DUMP_OPENLIST   true    //Dump la liste ouverte à chaque itération
#define PFINDER_DUMP_CLOSEDLIST true    //Dump la liste fermée  à chaque itération
#define PFINDER_DUMP_NEIGHLIST  true    //Dump la liste de voisinage à chaque itération
#endif

template<class m_elem> class pfNode;
template<class m_ctr,class m_elem> class pathfinder;

#if USE_INTERNAL_CONTAINER

#define Pathfinder  pathfinder<map_ctr<map_elm>,map_elm>
#define Map_ctr     map_ctr<map_elm>

class map_elm;
template<class m_elem> class map_ctr;

class map_elm{
    public:
        bool walkable;
        unsigned int X_coord;
        unsigned int Y_coord;

        //Calcule la distance heuristique à un autre point
        virtual unsigned int h_distance(map_elm* segend){
            //Manhattan sur distances x 10
            unsigned int dX = (X_coord > segend->X_coord)?X_coord-segend->X_coord:segend->X_coord-X_coord;
            unsigned int dY = (Y_coord > segend->Y_coord)?Y_coord-segend->Y_coord:segend->Y_coord-Y_coord;
            return (dX + dY)*10;
        }
};

template<class m_elem>
class map_ctr{

    public:
        m_elem * content;
        unsigned int width;
        unsigned int height;

        //Méthode de construction
        map_ctr(unsigned int w = 0,unsigned int h = 0): width(w), height(h){
            //Allocation de l'espace de la map
            //Tableau unidimentionnel, accès en X + width * Y
            if( height*width == 0 ){ content = NULL; return; }
            content = new m_elem[height*width];
        }
        //Méthode de destruction
        ~map_ctr(){
            //Désallocation de l'espace
            if( content ) delete[] content;
        }
        //Renvoie l'élément à une coordonnée donnée
        m_elem& at(unsigned int x,unsigned int y){
            return content[x + width*y];
        }

        //Vérifie l'existance d'une case
        bool exists(unsigned int x, unsigned int y){
            return !((x >= width)||(y >= height));
        }

        //Renvoie si l'élément est marchable
        virtual bool isWalkable(m_elem* elem){ return elem->walkable; }
        //Renvoie une liste d'éléments directement adjacents
        virtual void getNeighborhood( pfNode<m_elem>* node, list< pfNode<m_elem>* >* neighborhood){

            #if PFINDER_DEBUGMODE
            fprintf(PF_DFT_DEBUG_FDESCR,"Map_ctr Neighborhood seeking func Begin\n");
            #endif

            //Récupération des coordonnées de l'élément:
            unsigned int offset = (node->elem - content);
            unsigned int i = offset / width; //Position Y dans le tableau
            unsigned int j = offset % width; //Position X dans le tableau
            #if PFINDER_DEBUGMODE
            fprintf(PF_DFT_DEBUG_FDESCR,"Element computed position: o%d -> x%d y%d\n",offset,j,i);
            #endif

            //Ajout des nouveaux noeuds dans la liste de sortie
            for( int k = 0; k < 8; k++ ){
                switch(k){
                    default: continue;
                    case 0: if( i < 1 ){ //NORD
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"NORTH undefined\n");
                                #endif
                                continue;
                            }
                            if( isWalkable(&content[ (i-1)*width + j ]) )
                                neighborhood->push_back( new pfNode<m_elem>(&(content[ (i-1)*width + j ])) );
                            else {
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"NORTH unwalkable\n");
                                #endif
                                continue;
                            }
                            break;
                    case 1: if( i >= height-1 ){ //SUD
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"SOUTH undefined\n");
                                #endif
                                continue;
                            }
                            if( isWalkable(&content[ (i+1)*width + j ]) )
                                neighborhood->push_back( new pfNode<m_elem>(&(content[ (i+1)*width + j ])) );
                            else {
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"SOUTH unwalkable\n");
                                #endif
                                continue;
                            }
                            break;
                    case 2: if( j < 1 ){ //OUEST
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"WEST undefined\n");
                                #endif
                                continue;
                            }
                            if( isWalkable(&content[ i*width + j-1 ]) )
                                neighborhood->push_back( new pfNode<m_elem>(&(content[ i*width + j-1 ])) );
                            else {
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"WEST unwalkable\n");
                                #endif
                                continue;
                            }
                            break;
                    case 3: if( j >= width-1 ){  //EST
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"EAST undefined\n");
                                #endif
                                continue;
                            }
                            if( isWalkable(&content[ i*width + j+1 ]) )
                                neighborhood->push_back( new pfNode<m_elem>(&(content[ i*width + j+1 ])) );
                            else {
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"EAST unwalkable\n");
                                #endif
                                continue;
                            }
                            break;
                    case 4: if( j < 1 || i < 1 ){ //N-O
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"NORTH-WEST undefined\n");
                                #endif
                                continue;
                            }
                            if( isWalkable(&content[ (i-1)*width + j-1 ]) )
                                neighborhood->push_back( new pfNode<m_elem>(&(content[ (i-1)*width + j-1 ])) );
                            else {
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"NORTH-WEST unwalkable\n");
                                #endif
                                continue;
                            }
                            break;
                    case 5: if( j >= width-1 || i < 1 ){ //N-E
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"NORTH-EAST undefined\n");
                                #endif
                                continue;
                            }
                            if( isWalkable(&content[ (i-1)*width + j+1 ]) )
                                neighborhood->push_back( new pfNode<m_elem>(&(content[ (i-1)*width + j+1 ])) );
                            else {
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"NORTH-EAST unwalkable\n");
                                #endif
                                continue;
                            }
                            break;
                    case 6: if( j < 1 || i >= height-1 ){ //S-O
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"SOUTH-WEST undefined\n");
                                #endif
                                continue;
                            }
                            if( isWalkable(&content[ (i+1)*width + j-1 ]) )
                                neighborhood->push_back( new pfNode<m_elem>(&(content[ (i+1)*width + j-1 ])) );
                            else {
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"SOUTH-WEST unwalkable\n");
                                #endif
                                continue;
                            }
                            break;
                    case 7: if( j >= width-1 || i >= height-1 ){ //S-E
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"SOUTH-EAST undefined\n");
                                #endif
                                continue;
                            }
                            if( isWalkable(&content[ (i+1)*width + j+1 ]) )
                                neighborhood->push_back( new pfNode<m_elem>(&(content[ (i+1)*width + j+1 ])) );
                            else {
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"SOUTH-EAST unwalkable\n");
                                #endif
                                continue;
                            }
                            break;
                }
                neighborhood->back()->parent = node;
                //On est dans un cas ou le déplacement diagonal vaut le déplacement val/hal
                //if( i < 4 ) //directions horizontale/verticale
                    neighborhood->back()->legacy = node->legacy + 10;
                //else        //directions diagonales
                //neighborhood->back()->legacy = node->legacy + 14;
            }

            #if PFINDER_DEBUGMODE
            fprintf(PF_DFT_DEBUG_FDESCR,"Map_ctr Neighborhood seeking func End\n");
            #endif

        }
};

#endif

template<class m_elem>
class pfNode{
    public:
        m_elem* elem;
        pfNode* parent;
        unsigned int heuristic;
        unsigned int legacy;
        unsigned int score;

        //Eléments chelous à potentiométrer
        unsigned int danger;
        unsigned int value;

        pfNode(m_elem* m): elem(m), parent(NULL),heuristic(0), legacy(0), score(0), danger(0), value(0){}
        void setHeuristic(m_elem* destination){
            heuristic = elem->h_distance( destination );
        }
        void setScore(){
            score = heuristic + legacy + danger + value;
        }
};

template<class m_ctr,class m_elem>
class pathfinder{
    public:
        m_ctr*          map;
        m_elem*         start;
        m_elem*         end;

        pathfinder(m_ctr* m): map(m),start(NULL),end(NULL){}
        ~pathfinder(){}

        //Fonction d'execution du pathfinder
        bool compute(list< m_elem* >* result,float Dfactor,float Vfactor){

            #if PFINDER_DEBUGMODE
                #if PFINDER_TRACK_ITERATION
                    unsigned int dbg_itcount = 0;
                #endif
                #if PFINDER_DUMP_OPENLIST
                    FILE* dbg_olist_dump = fopen("olist_dump.txt","w");
                #endif
                #if PFINDER_DUMP_CLOSEDLIST
                    FILE* dbg_clist_dump = fopen("clist_dump.txt","w");
                #endif
                #if PFINDER_DUMP_NEIGHLIST
                    FILE* dbg_nlist_dump = fopen("nlist_dump.txt","w");
                #endif
            #endif

            //Si le départ ou l'arrivée n'ont pas été définis
            if( !start || !end ) return false;
            if( start == end ) return false;

            #if PFINDER_DEBUGMODE
            fprintf(PF_DFT_DEBUG_FDESCR,"Pathfinder Starting\n");
            #endif

            //On alloue les conteneurs temporaires
            // /!\ on utilise des pointeurs, penser à nettoyer après passage ;)
            list< pfNode<m_elem>* > openList;
            list< pfNode<m_elem>* > closedList;

            //On prépare le noeud de départ
            openList.push_front( new pfNode<m_elem>( start ) );
            openList.front()->setHeuristic( end );
            openList.front()->setScore();

            #if PFINDER_DEBUGMODE
            fprintf(PF_DFT_DEBUG_FDESCR,"Startnode inserted at (%d,%d)\n",start->X_coord,start->Y_coord);
            #endif

            //Noeud de l'étude actuel
            pfNode<m_elem>* focus = openList.front();
            while( focus->elem != end ){

                #if PFINDER_DEBUGMODE
                    #if PFINDER_TRACK_ITERATION
                        dbg_itcount ++;
                    #endif
                    #if PFINDER_DUMP_OPENLIST
                        fprintf(dbg_olist_dump,"Iteration %d:\n--------\n",dbg_itcount);
                        for( typename list< pfNode<m_elem>* >::iterator dbg_it = openList.begin(); dbg_it != openList.end(); dbg_it++ ){
                            fprintf(dbg_olist_dump,"[%d,%d](S:%d; H:%d; L:%d; P[%d,%d])\n",
                                    (*dbg_it)->elem->X_coord,
                                    (*dbg_it)->elem->Y_coord,
                                    (*dbg_it)->score,(*dbg_it)->heuristic,(*dbg_it)->legacy,
                                    ((*dbg_it)->parent)?(*dbg_it)->parent->elem->X_coord:0,
                                    ((*dbg_it)->parent)?(*dbg_it)->parent->elem->Y_coord:0);
                        }
                        fprintf(dbg_olist_dump,"--------\n");
                        fflush(dbg_olist_dump);
                    #endif
                fprintf(PF_DFT_DEBUG_FDESCR,"Focusing on (%d,%d):%d\n",focus->elem->X_coord,focus->elem->Y_coord,focus->score);
                #endif

                //On récupère la liste des noeuds à proximité
                #if PFINDER_DEBUGMODE
                fprintf(PF_DFT_DEBUG_FDESCR,"Seeking for neighborhood...\n");
                    #if PFINDER_DUMP_NEIGHLIST
                        fprintf(dbg_nlist_dump,"Iteration %d:\n--------\n",dbg_itcount);
                    #endif
                #endif
                list< pfNode<m_elem>* > neighborhood;
                map->getNeighborhood( focus, &neighborhood );

                //On traite les éléments obtenus en fabricant des noeuds ou en updatant les noeuds précédemment existants
                for( typename list<pfNode<m_elem>*>::iterator it = neighborhood.begin(); it != neighborhood.end(); it++ ){

                    bool inlist = false;

                    (*it)->setHeuristic( end );
                    (*it)->danger *= Dfactor;
                    (*it)->value *= Vfactor;
                    (*it)->setScore();

                    #if PFINDER_DEBUGMODE
                    fprintf(PF_DFT_DEBUG_FDESCR,"Neighbor (%d,%d):%d\n",(*it)->elem->X_coord,(*it)->elem->Y_coord,(*it)->score);
                        #if PFINDER_DUMP_NEIGHLIST
                            fprintf(dbg_nlist_dump,"[%d,%d](S:%d; H:%d; L:%d; P[%d,%d])\n",
                                        (*it)->elem->X_coord,
                                        (*it)->elem->Y_coord,
                                        (*it)->score,(*it)->heuristic,(*it)->legacy,
                                        ((*it)->parent)?(*it)->parent->elem->X_coord:0,
                                        ((*it)->parent)?(*it)->parent->elem->Y_coord:0);
                        #endif
                    #endif

                    //On vérifie la présence dans la liste ouverte
                    {
                        #if PFINDER_DEBUGMODE
                        fprintf(PF_DFT_DEBUG_FDESCR,"In open? ");
                        #endif
                        typename list< pfNode<m_elem>* >::iterator it2 = openList.begin();
                        for( it2++ ; it2 != openList.end(); it2++ ){
                            if( (*it2)->elem == (*it)->elem ){
                                #if PFINDER_DEBUGMODE
                                fprintf(PF_DFT_DEBUG_FDESCR,"Yes\nBetter Score? ");
                                    #if PFINDER_DUMP_NEIGHLIST
                                    fprintf(dbg_nlist_dump,"In open\n");
                                    #endif
                                #endif
                                //Doit-on mettre à jour le noeud?
                                if( (*it2)->score > (*it)->score ){
                                    #if PFINDER_DEBUGMODE
                                    fprintf(PF_DFT_DEBUG_FDESCR,"Yes\nSorting... ");
                                        #if PFINDER_DUMP_NEIGHLIST
                                        fprintf(dbg_nlist_dump,"Better Score\n");
                                        #endif
                                    #endif
                                    typename list< pfNode<m_elem>* >::iterator it3 = openList.begin();
                                    for( it3++; it3 != openList.end(); it3++ ){
                                        if((*it)->score < (*it3)->score) break;
                                    }
                                    #if PFINDER_DEBUGMODE
                                    if( it3 == openList.end() ){
                                        fprintf(PF_DFT_DEBUG_FDESCR,"@end\n");
                                        #if PFINDER_DUMP_NEIGHLIST
                                        fprintf(dbg_nlist_dump,"@end\n");
                                        #endif
                                    }
                                    else{
                                        fprintf(PF_DFT_DEBUG_FDESCR,"before (%d %d):%d\n",(*it3)->elem->X_coord,(*it3)->elem->Y_coord,(*it3)->score);
                                        #if PFINDER_DUMP_NEIGHLIST
                                        fprintf(dbg_nlist_dump,"bef (%d %d):%d\n",(*it3)->elem->X_coord,(*it3)->elem->Y_coord,(*it3)->score);
                                        #endif
                                    }
                                    #endif
                                    openList.insert(it3,*it);
                                    delete *it2;
                                    openList.erase(it2);
                                }else{
                                    #if PFINDER_DEBUGMODE
                                    fprintf(PF_DFT_DEBUG_FDESCR,"No\n");
                                    #endif
                                }
                                //On peut s'arrêter là
                                inlist = true;
                                break;
                            }
                        }
                    }
                    if( inlist ){
                        continue; //idem
                    }
                    else{
                        #if PFINDER_DEBUGMODE
                        fprintf(PF_DFT_DEBUG_FDESCR,"No. Continue...\n");
                        #endif
                    }

                    #if PFINDER_DEBUGMODE
                    fprintf(PF_DFT_DEBUG_FDESCR,"In closed? ");
                    #endif
                    //On vérifie la présence dans la liste fermée
                    for( typename list< pfNode<m_elem>* >::reverse_iterator it2 = closedList.rbegin(); it2 != closedList.rend(); it2++ ){
                        if( (*it2)->elem == (*it)->elem ){
                            #if PFINDER_DEBUGMODE
                            fprintf(PF_DFT_DEBUG_FDESCR,"Yes\n");
                                #if PFINDER_DUMP_NEIGHLIST
                                fprintf(dbg_nlist_dump,"In closed\n");
                                #endif
                            #endif
                            //On peut s'arrêter là
                            inlist = true;
                            break;
                        }
                    }
                    if( inlist ){
                        delete *it;
                        continue; //idem
                    }
                    else{
                        #if PFINDER_DEBUGMODE
                        fprintf(PF_DFT_DEBUG_FDESCR,"No. Continue\n");
                        #endif
                    }

                    {
                        #if PFINDER_DEBUGMODE
                        fprintf(PF_DFT_DEBUG_FDESCR,"Inserting... ");
                        #endif
                        typename list< pfNode<m_elem>* >::iterator it2 = openList.begin();
                        for( it2++; it2 != openList.end(); it2++ ){
                            if((*it)->score < (*it2)->score){
                                #if PFINDER_DUMP_NEIGHLIST
                                fprintf(dbg_nlist_dump,"BREAK\n");
                                #endif
                                break;
                            }
                        }
                        #if PFINDER_DEBUGMODE
                        if( it2 == openList.end() ){
                            fprintf(PF_DFT_DEBUG_FDESCR,"@end\n");
                            #if PFINDER_DUMP_NEIGHLIST
                            fprintf(dbg_nlist_dump,"@end\n");
                            #endif
                        }
                        else{
                            fprintf(PF_DFT_DEBUG_FDESCR,"before (%d %d):%d\n",(*it2)->elem->X_coord,(*it2)->elem->Y_coord,(*it2)->score);
                            #if PFINDER_DUMP_NEIGHLIST
                            fprintf(dbg_nlist_dump,"bef (%d %d):%d\n",(*it2)->elem->X_coord,(*it2)->elem->Y_coord,(*it2)->score);
                            #endif
                        }
                        #endif
                        openList.insert(it2,*it);
                    }

                }

                #if PFINDER_DUMP_NEIGHLIST
                    fprintf(dbg_nlist_dump,"--------\n");
                    fflush(dbg_nlist_dump);
                #endif

                //On ajoute ensuite le noeud dans la closedList
                closedList.push_back( focus );

                #if PFINDER_DUMP_CLOSEDLIST
                    fprintf(dbg_clist_dump,"Iteration %d:\n--------\n",dbg_itcount);
                    for( typename list< pfNode<m_elem>* >::iterator dbg_it = closedList.begin(); dbg_it != closedList.end(); dbg_it++ ){
                        fprintf(dbg_clist_dump,"[%d,%d](S:%d; H:%d; L:%d; P[%d,%d])\n",
                                (*dbg_it)->elem->X_coord,
                                (*dbg_it)->elem->Y_coord,
                                (*dbg_it)->score,(*dbg_it)->heuristic,(*dbg_it)->legacy,
                                ((*dbg_it)->parent)?(*dbg_it)->parent->elem->X_coord:0,
                                ((*dbg_it)->parent)?(*dbg_it)->parent->elem->Y_coord:0);
                    }
                    fprintf(dbg_clist_dump,"--------\n");
                    fflush(dbg_clist_dump);
                #endif

                //On passe à l'élément suivant ou bien on sort de la boucle
                openList.pop_front();
                if( openList.empty() ) break;
                focus = openList.front();

            }

            //A-t-on effectué le travail correctement?
            if( focus->elem != end ){
                //Cleanup
                for( typename list< pfNode< m_elem >* >::iterator it = openList.begin(); it != openList.end(); it++ ) delete *it;
                for( typename list< pfNode< m_elem >* >::iterator it = closedList.begin(); it != closedList.end(); it++ ) delete *it;
                return false;
            }

            //On reconstruit le résultat
            do{
                result->push_front( focus->elem );
                focus = focus->parent;
            }while( focus->parent != NULL );
            result->push_front( focus->elem );

            //Cleanup
            for( typename list< pfNode< m_elem >* >::iterator it = openList.begin(); it != openList.end(); it++ ) delete *it;
            for( typename list< pfNode< m_elem >* >::iterator it = closedList.begin(); it != closedList.end(); it++ ) delete *it;
            return true;

        }

};

#endif
